/*
 * Main_User.c
 *
 * Created  : 9 November 2020
 * Modified : 7 April 2021
 * Author   : HWanyusof
 * Version	: 1.0
 */

#include "usbd_cdc_if.h"
#include "string.h"
#include "stm32f4xx_hal.h"
#include "VCNL4035X01.h"
#include "VCNL4035X01_Prototypes.h"
#include "typedefinition.h"
#include "VCNL4035X01_Application_Library.h"
#include "Main_User.h"


//Global Variables
int SEL_PS_Mode;
int SEL_ALS_Mode;
int SEL_Print_Mode;
Byte ALS_IT;
Byte PS_IT;
int IRED_Channel;
int IRED_Channel_Offset_Read;
int SEL_Offset;

//Switch the I2C Bus number here
int I2C_Bus = 2;

//Variables for Offset Value
int CalibValue = 0;
int OffsetValue = 0;
int AverageCount = 10; //Change the average count to the needed number of offset measurement
int Max_Offset = 0;

void INIT_VCNL4035X01()
{
	/* Reset Sensor to default value */
	Reset_Sensor();

	/****************************************************************************/
	/***********************Setting Up The Mode & Variables**********************/

	/* Choose a desired proximity and light sensing mode (ALS or White Channel) */
	/*
	 * SEL_PS_Mode - Select the proximity mode:
	 * 0 - Turn Off proximity mode/shutdown
	 * 1 - Auto/Self-Timed Mode
	 * 2 - Active Force Mode/Continuous Forced Mode
	 * 3 - Gesture Mode
	 * 4 - Low Power Mode
	 *
	 * SEL_ALS_Mode - Select the light sensing mode (ALS or White Channel):
	 * 0 - Turn Off ALS mode/shutdown
	 * 1 - ALS Mode
	 * 2 - White Channel Mode
	 */
	SEL_PS_Mode = 4;
	SEL_ALS_Mode = 1;

	/* Choose the print mode */
	/*
	 * SEL_Print_Mode - Select the print mode:
	 * 1 - Print individual mode output data (PS data or ALS data)
	 * 2 - Print all of the register values (Including PS and ALS output data registers)
	 * 3 - Print all of the sensor register settings
	 *
	 * Note : If print mode 1 is selected, it is recommended to use either PS or ALS mode at a time and switch off the other.
	 * If both modes activated at the same time, the interrupt bit changes could appear in different mode print function due
	 * to delay function, causing confusion.
	 */
	SEL_Print_Mode = 2;

	/* Choose the integration time for ALS and white mode */
	// The rest of sensor parameters can be set individually in each of the mode functions below.
	// The reason for global variable is because of the dependency of the same variable at different places in the code.
	/*
	 * ALS_IT - Select the integration time for ALS and white channel measurement:
	 * VCNL4035X01_ALS_IT_50MS - Shortest IT is suitable for bright condition
	 * VCNL4035X01_ALS_IT_100MS
	 * VCNL4035X01_ALS_IT_200MS
	 * VCNL4035X01_ALS_IT_400MS
	 * VCNL4035X01_ALS_IT_800MS - Longest IT is suitable for dark condition
	 *
	 */
	ALS_IT = VCNL4035X01_ALS_IT_50MS;

	/* Choose the integration time for PS */
	// The rest of sensor parameters can be set individually in each of the mode functions below.
	// The reason for global variable is because of the dependency of the same variable at different places in the code.
	/*
	 * PS_IT - Select the integration time for proximity measurement for Auto, AF, Low Power and Gesture Mode:
	 * VCNL4035X01_PS_IT_1T - Shortest IT is suitable for short distance range
     * VCNL4035X01_PS_IT_1_5T
     * VCNL4035X01_PS_IT_2T
     * VCNL4035X01_PS_IT_2_5T
     * VCNL4035X01_PS_IT_3T
     * VCNL4035X01_PS_IT_3_5T
     * VCNL4035X01_PS_IT_4T
     * VCNL4035X01_PS_IT_8T - Longest IT is suitable for extended distance range
	 *
	 */
	PS_IT = VCNL4035X01_PS_IT_2T;

	/* Choose the IRED channel for PS */
	// The rest of sensor parameters can be set individually in each of the mode functions below.
	/*
	 * IRED_Channel - Select the IRED channel for proximity measurement for Auto, AF and Low Power Mode:
	 * VCNL4035X01_PS_IRED_1
	 * VCNL4035X01_PS_IRED_2
	 * VCNL4035X01_PS_IRED_3
	 *
	 */
	IRED_Channel = VCNL4035X01_PS_IRED_3;

	/* Choose to turn on/off offset cancellation measurement */
	/* SEL_Offset - Select the offset mode:
	 * 0 - Turn Off offset cancellation measurement
	 * 1 - Turn On offset cancellation measurement
	 */
	SEL_Offset = 1;

	/*******************End of Setting Up The Mode & Variables*******************/
	/****************************************************************************/

	/****************************************************************************/
	/**********************Implementation of above settings**********************/

	/* Set the Data channel to read the offset */
	switch(IRED_Channel)
	{
		case VCNL4035X01_PS_IRED_1 : IRED_Channel_Offset_Read = VCNL4035X01_PS_DATA_1;
									 break;
		case VCNL4035X01_PS_IRED_2 : IRED_Channel_Offset_Read = VCNL4035X01_PS_DATA_2;
									 break;
		case VCNL4035X01_PS_IRED_3 : IRED_Channel_Offset_Read = VCNL4035X01_PS_DATA_3;
									 break;
		default : break;
	}

	/* Set the proximity mode */
	switch(SEL_PS_Mode)
	{
		case 1  : Auto_Mode(); //Also known as Self-Timed Mode
				  break;
		case 2  : AF_Mode(); //Also known Continuous Forced Mode (Could also be used for single forced mode)
		          break;
		case 3  : Gesture_Mode();
				  break;
		case 4  : Low_Power_Mode(); //Auto/Self-Timed Mode with 1/10 of normal LED current
				  break;
		default : break;
	}

	/* Set the light sensing mode */
	switch(SEL_ALS_Mode)
	{
		case 1  : ALS_Mode();
		          break;
		case 2  : White_Mode();
				  break;
		default : break;
	}

	/***************************End of Implementation****************************/
    /****************************************************************************/
}

//Print Function
void PRINT_VCNL4035X01()
{
	/* Print the output based on the mode */
	switch(SEL_Print_Mode)
	{
		case 1 : 	switch(SEL_PS_Mode)
					{
						case 1  : Print_Auto_Mode();
								  break;
						case 2  : Print_AF_Mode();
								  break;
						case 3  : Print_Gesture_Mode();
								  break;
						case 4  : Print_Low_Power_Mode();
								  break;
						default : break;
					}

					switch(SEL_ALS_Mode)
					{
						case 1  : Print_ALS_Mode(ALS_IT);
								  break;
						case 2  : Print_White_Mode(ALS_IT);
								  break;
						default : break;
					}

					break;

		case 2 :	Print_All(ALS_IT);
					break;

		case 3 :	Print_Register_Settings();
					break;

		default :   break;
	}
}

//White Mode Initialization Function
void White_Mode()
{
    //1.)Initialization
    //Switch Off the ALS
    VCNL4035X01_SET_ALS_SD(VCNL4035X01_ALS_SD_DIS);
    //Disable White Channel
    VCNL4035X01_SET_WHITE_SD(VCNL4035X01_ALS_WHITE_SD_DIS);

    //2.) Setting up White Channel
    //Can use ALS channel parameters
    //ALS_CONF1
    //Enable/Disable Interrupt
    VCNL4035X01_SET_ALS_Interrupt(VCNL4035X01_ALS_INT_EN);
    //Set the Persistence of the Ambient Light Sensor
    VCNL4035X01_SET_ALS_PERS(VCNL4035X01_ALS_PERS_1);
    //Set the Dynamic Range
    VCNL4035X01_SET_ALS_HD(VCNL4035X01_ALS_HD_X1);
    //Set the ALS Integration Time
    VCNL4035X01_SET_ALS_IT(ALS_IT);

    //ALS_CONF2
    //Set the ALS Sensitivity
    VCNL4035X01_SET_ALS_NS(VCNL4035X01_ALS_NS_X2);

    //ALS_THDH
    //Set the ALS Interrupt Higher Threshold
    VCNL4035X01_SET_ALS_HighThreshold(5000);

    //ALS_THDL
    //Set the ALS Interrupt Lower Threshold
    VCNL4035X01_SET_ALS_LowThreshold(3000);

    //3.) Switch On the White and ALS Channel
    VCNL4035X01_SET_ALS_SD(VCNL4035X01_ALS_SD_EN);
    VCNL4035X01_SET_WHITE_SD(VCNL4035X01_ALS_WHITE_SD_EN);

    HAL_Delay(1000);
}

//ALS Mode Initialization Function
void ALS_Mode()
{
    //1.)Initialization
    //Switch Off the ALS
    VCNL4035X01_SET_ALS_SD(VCNL4035X01_ALS_SD_DIS);
    //Enable/Disable White Channel (Disable for normal ALS wavelength)
    VCNL4035X01_SET_WHITE_SD(VCNL4035X01_ALS_WHITE_SD_DIS);

    //2.) Setting up ALS
    //ALS_CONF1
    //Enable/Disable Interrupt
    VCNL4035X01_SET_ALS_Interrupt(VCNL4035X01_ALS_INT_EN);
    //Set the Persistence of the Ambient Light Sensor
    VCNL4035X01_SET_ALS_PERS(VCNL4035X01_ALS_PERS_1);
    //Set the Dynamic Range
    VCNL4035X01_SET_ALS_HD(VCNL4035X01_ALS_HD_X1);
    //Set the ALS Integration Time
    VCNL4035X01_SET_ALS_IT(ALS_IT);

    //ALS_CONF2
    //Set the ALS Sensitivity
    VCNL4035X01_SET_ALS_NS(VCNL4035X01_ALS_NS_X2);

    //ALS_THDH
    //Set the ALS Interrupt Higher Threshold
    VCNL4035X01_SET_ALS_HighThreshold(5000);

    //ALS_THDL
    //Set the ALS Interrupt Lower Threshold
    VCNL4035X01_SET_ALS_LowThreshold(3000);

    //3.) Switch On the ALS
    VCNL4035X01_SET_ALS_SD(VCNL4035X01_ALS_SD_EN);

    //Clear Initial Interrupt
    VCNL4035X01_GET_PS_Interrupt();

    HAL_Delay(1000);
}

//Low Power Mode Initialization Function
void Low_Power_Mode()
{
    //1.)Initialization
    //Disable the PS
    VCNL4035X01_SET_PS_SD(VCNL4035X01_PS_SD_DIS);
    //Setting up Auto Mode
    VCNL4035X01_SET_PS_AF(VCNL4035X01_PS_AUTO);
    //PS_SD will be set before measuring the offset or after all of the parameters have been set up

    //2.) Setting up PS
    //PS_CONF1
    //Set the PS IRED on/off Duty Ratio setting
    VCNL4035X01_SET_PS_DUTY(VCNL4035X01_PS_DR_1_40);
    //Set the Persistence
    VCNL4035X01_SET_PS_PERS(VCNL4035X01_PS_PERS_1);
    //Set the Integration Time
    VCNL4035X01_SET_PS_IT(PS_IT);

    //PS_CONF2
    //Enable/Disable the Gesture Interrupt
    VCNL4035X01_SET_GESTURE_INT(VCNL4035X01_PS_GESTURE_INT_DIS);
    //Enable/Disable the Gesture Mode
    VCNL4035X01_SET_GESTURE_MODE(VCNL4035X01_PS_GESTURE_MODE_DIS);
    //Set the Gain
    VCNL4035X01_SET_PS_Gain(VCNL4035X01_PS_GAIN_2_STEP);
    //Set the Output Bit Size
    VCNL4035X01_SET_PS_HD(VCNL4035X01_PS_HD_16Bits);
    //Set the Sensitivity Mode
    VCNL4035X01_SET_PS_NS(VCNL4035X01_PS_NS_2STEP_MODE_X4);
    //Set the Interrupt
    VCNL4035X01_SET_PS_INT(VCNL4035X01_PS_INT_CLOSE_AWAY);

    //PS_CONF3
    //Enable/Disable Low Current
    //When enabled -  1/10 of normal current,
    //with that the current is accordingly: 5 mA, 7.5 mA, 10 mA, 12 mA, 14 mA, 16 mA, 18 mA, 20 mA.
    //The current in VCNL4035X01_SET_PS_LED_I() still has to be set and the value will be 1/10 of that.
    VCNL4035X01_SET_PS_LED_I_LOW(VCNL4035X01_PS_I_LOW_EN);
    //Select IRED input to be Driven by the Internal Driver
    VCNL4035X01_SET_PS_IRED_select(IRED_Channel);
    //Enable/Disable Smart Persistence
    VCNL4035X01_SET_PS_SMART_PERS(VCNL4035X01_PS_SMART_PERS_DIS);
    //Set Interrupt to Normal/Logic Mode
    VCNL4035X01_SET_PS_MS(VCNL4035X01_PS_MS_NORMAL);
    //Enable/Disable Sunlight Cancellation
    VCNL4035X01_SET_PS_SC_EN(VCNL4035X01_PS_SC_EN);
    //PS_AF has been set during initialization
    //PS_TRIG must be set before reading the proximity data

    //PS_MS
    //Set the Sunlight Cancellation Current
    VCNL4035X01_SET_PS_SC_CUR(VCNL4035X01_PS_SC_CURx1);
    //Set the Sunlight Capability
    VCNL4035X01_SET_PS_SP(VCNL4035X01_PS_SPx1);
    //Set the Output of Sunlight Protect Mode
    VCNL4035X01_SET_PS_SPO(VCNL4035X01_PS_SPO_00);
    //Set the LED Current
    VCNL4035X01_SET_PS_LED_I(VCNL4035X01_PS_LED_I_200mA);

    //3.) Switch On the sensor
    VCNL4035X01_SET_PS_SD(VCNL4035X01_PS_SD_EN);
    //Delay of 10 ms needs to be changed depending on the API of the µ-controller of use
    HAL_Delay(10);

    //Clear Initial Interrupt
    VCNL4035X01_GET_PS_Interrupt();
    HAL_Delay(10);

	//4.) Threshold Setting and Offset Measurement
	for(int i=0;i<AverageCount;i++)
	{
		CalibValue += VCNL4035X01_READ_Reg(IRED_Channel_Offset_Read);
		//Delay of 10 ms needs to be changed depending on the API of the µ-controller of use
		HAL_Delay(10);
	}

	//Calculate the average of the offset measurement
	CalibValue /= AverageCount;

	//Check whether the offset value is too high to have been set in the cancellation register
	// 1T max offset value for the cancellation register is 2048
	if((PS_IT==VCNL4035X01_PS_IT_1T)||(PS_IT==VCNL4035X01_PS_IT_1_5T))Max_Offset = 2048;
	// 2T max offset value for the cancellation register is 4096
	if((PS_IT==VCNL4035X01_PS_IT_2T)||(PS_IT==VCNL4035X01_PS_IT_2_5T)||(PS_IT==VCNL4035X01_PS_IT_3T)||(PS_IT==VCNL4035X01_PS_IT_3_5T))Max_Offset = 4096;
	// 4T max offset value for the cancellation register is 8192
	if(PS_IT==VCNL4035X01_PS_IT_4T)Max_Offset = 8192;
	// 8T max offset value for the cancellation register is 16384
	if(PS_IT==VCNL4035X01_PS_IT_8T)Max_Offset = 16384;
	if(CalibValue>Max_Offset)
	{
		//Perform Offset Measurement
		if(SEL_Offset == 0) OffsetValue = 0;
		if(SEL_Offset == 1) OffsetValue = CalibValue;
		//Set Cancellation register to eliminate offset
		VCNL4035X01_SET_PS_CANC(OffsetValue);
		//Set Low Threshold
		VCNL4035X01_SET_PS_LowThreshold(3000 + OffsetValue);
		//set High Threshold
		VCNL4035X01_SET_PS_HighThreshold(5000 + OffsetValue);
	}
	else
	{
		//Perform Offset Measurement
		if(SEL_Offset == 0) OffsetValue = 0;
		if(SEL_Offset == 1) OffsetValue = CalibValue;
		//Set Cancellation register to eliminate offset
		VCNL4035X01_SET_PS_CANC(OffsetValue);
		//Set Low Threshold
		VCNL4035X01_SET_PS_LowThreshold(3000);
		//set High Threshold
		VCNL4035X01_SET_PS_HighThreshold(5000);
	}

    HAL_Delay(1000);
}

//Gesture Mode Initialization Function
void Gesture_Mode()
{
    //1.)Initialization
    //Disable the PS
    VCNL4035X01_SET_PS_SD(VCNL4035X01_PS_SD_DIS);
    //Setting up AF Mode (Needed for Gesture Mode)
    VCNL4035X01_SET_PS_AF(VCNL4035X01_PS_AF_EN);

    //2.) Setting up PS
    //PS_CONF1
    //PS_DUTY does't play a role in Gesture Mode
    //Set the Persistence
    VCNL4035X01_SET_PS_PERS(VCNL4035X01_PS_PERS_1);
    //Set the Integration Time
    VCNL4035X01_SET_PS_IT(PS_IT);

    //PS_CONF2
    //Enable/Disable the Gesture Interrupt
    VCNL4035X01_SET_GESTURE_INT(VCNL4035X01_PS_GESTURE_INT_EN);
    //Disable the Gesture Mode (Enable only after offset measurement)
    //If the mode be enabled now then only one channel will be measured
    VCNL4035X01_SET_GESTURE_MODE(VCNL4035X01_PS_GESTURE_MODE_DIS);
    //Set the Gain
    VCNL4035X01_SET_PS_Gain(VCNL4035X01_PS_GAIN_2_STEP);
    //Set the Output Bit Size
    VCNL4035X01_SET_PS_HD(VCNL4035X01_PS_HD_16Bits);
    //Set the Sensitivity Mode
    VCNL4035X01_SET_PS_NS(VCNL4035X01_PS_NS_2STEP_MODE_X4);
    //Set the Interrupt
    VCNL4035X01_SET_PS_INT(VCNL4035X01_PS_INT_CLOSE_AWAY);

    //PS_CONF3
    //Enable/Disable Low Current
    VCNL4035X01_SET_PS_LED_I_LOW(VCNL4035X01_PS_I_LOW_DIS);
    //Select IRED input to be Driven by the Internal Driver (For Gesture Mode not so important since all 3 channels will be read)
    VCNL4035X01_SET_PS_IRED_select(IRED_Channel);
    //Enable/Disable Smart Persistence
    VCNL4035X01_SET_PS_SMART_PERS(VCNL4035X01_PS_SMART_PERS_DIS);
    //Set Interrupt to Normal/Logic Mode
    VCNL4035X01_SET_PS_MS(VCNL4035X01_PS_MS_NORMAL);
    //Enable/Disable Sunlight Cancellation
    VCNL4035X01_SET_PS_SC_EN(VCNL4035X01_PS_SC_EN);
    //PS_AF has been set during initialization
    //PS_TRIG must be set before reading the proximity data

    //PS_MS
    //Set the Sunlight Cancellation Current
    VCNL4035X01_SET_PS_SC_CUR(VCNL4035X01_PS_SC_CURx1);
    //Set the Sunlight Capability
    VCNL4035X01_SET_PS_SP(VCNL4035X01_PS_SPx1);
    //Set the Output of Sunlight Protect Mode
    VCNL4035X01_SET_PS_SPO(VCNL4035X01_PS_SPO_00);
    //Set the LED Current
    VCNL4035X01_SET_PS_LED_I(VCNL4035X01_PS_LED_I_100mA);

    //3.) Switch On the sensor
    VCNL4035X01_SET_PS_SD(VCNL4035X01_PS_SD_EN);
    //Delay of 10 ms needs to be changed depending on the API of the µ-controller of use
    HAL_Delay(10);

    //Clear Initial Interrupt
    VCNL4035X01_GET_PS_Interrupt();
    HAL_Delay(10);

	//4.) Threshold Setting and Offset Measurement
	//Calculate Offset for all of the 3 channels
	for(int i=0; i<AverageCount;i++)
	{
		//Channel 1
		//Select IRED input to be driven by the internal driver to calibrate
		VCNL4035X01_SET_PS_IRED_select(VCNL4035X01_PS_IRED_1);
		//Delay of 10 ms needs to be changed depending on the API of the µ-controller of use
		HAL_Delay(10);
		//Enable trigger to start offset measurement
		VCNL4035X01_SET_PS_TRIG(VCNL4035X01_PS_TRIG_EN);
		//Delay of 10 ms needs to be changed depending on the API of the µ-controller of use
		HAL_Delay(10);
		//Read offset data channel 1 and add as calibration value for offset
		CalibValue += VCNL4035X01_READ_Reg(VCNL4035X01_PS_DATA_1);
		//Delay of 10 ms needs to be changed depending on the API of the µ-controller of use
		HAL_Delay(10);

		//Channel 2
		//Select IRED input to be driven by the internal driver to calibrate
		VCNL4035X01_SET_PS_IRED_select(VCNL4035X01_PS_IRED_2);
		//Delay of 10 ms needs to be changed depending on the API of the µ-controller of use
		HAL_Delay(10);
		//Enable trigger to start offset measurement
		VCNL4035X01_SET_PS_TRIG(VCNL4035X01_PS_TRIG_EN);
		//Delay of 10 ms needs to be changed depending on the API of the µ-controller of use
		HAL_Delay(10);
		//Read offset data channel 2 and add as calibration value for offset
		CalibValue += VCNL4035X01_READ_Reg(VCNL4035X01_PS_DATA_2);
		//Delay of 10 ms needs to be changed depending on the API of the µ-controller of use
		HAL_Delay(10);

		//Channel 3
		//Select IRED input to be driven by the internal driver to calibrate
		VCNL4035X01_SET_PS_IRED_select(VCNL4035X01_PS_IRED_3);
		//Delay of 10 ms needs to be changed depending on the API of the µ-controller of use
		HAL_Delay(10);
		//Enable trigger to start offset measurement
		VCNL4035X01_SET_PS_TRIG(VCNL4035X01_PS_TRIG_EN);
		//Delay of 10 ms needs to be changed depending on the API of the µ-controller of use
		HAL_Delay(10);
		//Read offset data channel 3 and add as calibration value for offset
		CalibValue += VCNL4035X01_READ_Reg(VCNL4035X01_PS_DATA_3);
		//Delay of 10 ms needs to be changed depending on the API of the µ-controller of use
		HAL_Delay(10);
	}

	//Calculate the average of the offset measurement from the 3 channels
	CalibValue /= 3*AverageCount;

	//Check whether the offset value is too high to be set in the cancellation register
	// 1T max offset value for the cancellation register is 2048
	if((PS_IT==VCNL4035X01_PS_IT_1T)||(PS_IT==VCNL4035X01_PS_IT_1_5T))Max_Offset = 2048;
	// 2T max offset value for the cancellation register is 4096
	if((PS_IT==VCNL4035X01_PS_IT_2T)||(PS_IT==VCNL4035X01_PS_IT_2_5T)||(PS_IT==VCNL4035X01_PS_IT_3T)||(PS_IT==VCNL4035X01_PS_IT_3_5T))Max_Offset = 4096;
	// 4T max offset value for the cancellation register is 8192
	if(PS_IT==VCNL4035X01_PS_IT_4T)Max_Offset = 8192;
	// 8T max offset value for the cancellation register is 16384
	if(PS_IT==VCNL4035X01_PS_IT_8T)Max_Offset = 16384;

	if(CalibValue>Max_Offset)
	{
		//Perform Offset Measurement
		if(SEL_Offset == 0) OffsetValue = 0;
		if(SEL_Offset == 1) OffsetValue = CalibValue;
		//Set Cancellation register to eliminate offset
		VCNL4035X01_SET_PS_CANC(OffsetValue);
		//Set Low Threshold
		VCNL4035X01_SET_PS_LowThreshold(3000 + OffsetValue);
		//set High Threshold
		VCNL4035X01_SET_PS_HighThreshold(5000 + OffsetValue);
	}
	else
	{
		//Perform Offset Measurement
		if(SEL_Offset == 0) OffsetValue = 0;
		if(SEL_Offset == 1) OffsetValue = CalibValue;
		//Set Cancellation register to eliminate offset
		VCNL4035X01_SET_PS_CANC(OffsetValue);
		//Set Low Threshold
		VCNL4035X01_SET_PS_LowThreshold(3000);
		//set High Threshold
		VCNL4035X01_SET_PS_HighThreshold(5000);
	}

    //5.) Enable the Gesture Mode
    //Enable/Disable the Gesture Mode
    VCNL4035X01_SET_GESTURE_MODE(VCNL4035X01_PS_GESTURE_MODE_EN);

    HAL_Delay(1000);
}

//AF/Continuous Forced Mode Initialization Function
void AF_Mode()
{
    //1.)Initialization
    //Disable the PS
    VCNL4035X01_SET_PS_SD(VCNL4035X01_PS_SD_DIS);
    //Setting up AF Mode
    VCNL4035X01_SET_PS_AF(VCNL4035X01_PS_AF_EN);
    //PS_SD will be set before measuring the offset or after all of the parameters have been set up

    //2.) Setting up PS
    //PS_CONF1
    //PS_DUTY does not play a role in AF Mode
    //Set the Persistence
    VCNL4035X01_SET_PS_PERS(VCNL4035X01_PS_PERS_1);
    //Set the Integration Time
    VCNL4035X01_SET_PS_IT(PS_IT);

    //PS_CONF2
    //Enable/Disable the Gesture Interrupt
    VCNL4035X01_SET_GESTURE_INT(VCNL4035X01_PS_GESTURE_INT_DIS);
    //Enable/Disable the Gesture Mode
    VCNL4035X01_SET_GESTURE_MODE(VCNL4035X01_PS_GESTURE_MODE_DIS);
    //Set the Gain
    VCNL4035X01_SET_PS_Gain(VCNL4035X01_PS_GAIN_2_STEP);
    //Set the Output Bit Size
    VCNL4035X01_SET_PS_HD(VCNL4035X01_PS_HD_16Bits);
    //Set the Sensitivity Mode
    VCNL4035X01_SET_PS_NS(VCNL4035X01_PS_NS_2STEP_MODE_X4);
    //Set the Interrupt
    VCNL4035X01_SET_PS_INT(VCNL4035X01_PS_INT_CLOSE_AWAY);

    //PS_CONF3
    //Enable/Disable Low Current
    VCNL4035X01_SET_PS_LED_I_LOW(VCNL4035X01_PS_I_LOW_DIS);
    //Select IRED input to be Driven by the Internal Driver
    VCNL4035X01_SET_PS_IRED_select(IRED_Channel);
    //Enable/Disable Smart Persistence
    VCNL4035X01_SET_PS_SMART_PERS(VCNL4035X01_PS_SMART_PERS_DIS);
    //Set Interrupt to Normal/Logic Mode
    VCNL4035X01_SET_PS_MS(VCNL4035X01_PS_MS_NORMAL);
    //Enable/Disable Sunlight Cancellation
    VCNL4035X01_SET_PS_SC_EN(VCNL4035X01_PS_SC_EN);
    //PS_AF has been set during initialization
    //PS_TRIG must be set before reading the proximity data

    //PS_MS
    //Set the Sunlight Cancellation Current
    VCNL4035X01_SET_PS_SC_CUR(VCNL4035X01_PS_SC_CURx1);
    //Set the Sunlight Capability
    VCNL4035X01_SET_PS_SP(VCNL4035X01_PS_SPx1);
    //Set the Output of Sunlight Protect Mode
    VCNL4035X01_SET_PS_SPO(VCNL4035X01_PS_SPO_00);
    //Set the LED Current
    VCNL4035X01_SET_PS_LED_I(VCNL4035X01_PS_LED_I_100mA);

    //3.) Switch On the sensor
    VCNL4035X01_SET_PS_SD(VCNL4035X01_PS_SD_EN);
    //Delay of 10 ms needs to be changed depending on the API of the µ-controller of use
    HAL_Delay(10);

    //Clear Initial Interrupt
    VCNL4035X01_GET_PS_Interrupt();
    HAL_Delay(10);

	//4.) Threshold Setting and Offset Measurement
	for(int i=0;i<AverageCount;i++)
	{
		//Enable trigger to start offset measurement
		VCNL4035X01_SET_PS_TRIG(VCNL4035X01_PS_TRIG_EN);
		//Delay of 10 ms needs to be changed depending on the API of the µ-controller of use
		HAL_Delay(10);
		CalibValue += VCNL4035X01_READ_Reg(IRED_Channel_Offset_Read);
		//Delay of 10 ms needs to be changed depending on the API of the µ-controller of use
		HAL_Delay(10);
	}

	//Calculate the average of the offset measurement
	CalibValue /= AverageCount;

	//Check whether the offset value is too high to have been set in the cancellation register
	// 1T max offset value for the cancellation register is 2048
	if((PS_IT==VCNL4035X01_PS_IT_1T)||(PS_IT==VCNL4035X01_PS_IT_1_5T))Max_Offset = 2048;
	// 2T max offset value for the cancellation register is 4096
	if((PS_IT==VCNL4035X01_PS_IT_2T)||(PS_IT==VCNL4035X01_PS_IT_2_5T)||(PS_IT==VCNL4035X01_PS_IT_3T)||(PS_IT==VCNL4035X01_PS_IT_3_5T))Max_Offset = 4096;
	// 4T max offset value for the cancellation register is 8192
	if(PS_IT==VCNL4035X01_PS_IT_4T)Max_Offset = 8192;
	// 8T max offset value for the cancellation register is 16384
	if(PS_IT==VCNL4035X01_PS_IT_8T)Max_Offset = 16384;
	if(CalibValue>Max_Offset)
	{
		//Perform Offset Measurement
		if(SEL_Offset == 0) OffsetValue = 0;
		if(SEL_Offset == 1) OffsetValue = CalibValue;
		//Set Cancellation register to eliminate offset
		VCNL4035X01_SET_PS_CANC(OffsetValue);
		//Set Low Threshold
		VCNL4035X01_SET_PS_LowThreshold(3000 + OffsetValue);
		//set High Threshold
		VCNL4035X01_SET_PS_HighThreshold(5000 + OffsetValue);
	}
	else
	{
		//Perform Offset Measurement
		if(SEL_Offset == 0) OffsetValue = 0;
		if(SEL_Offset == 1) OffsetValue = CalibValue;
		//Set Cancellation register to eliminate offset
		VCNL4035X01_SET_PS_CANC(OffsetValue);
		//Set Low Threshold
		VCNL4035X01_SET_PS_LowThreshold(3000);
		//set High Threshold
		VCNL4035X01_SET_PS_HighThreshold(5000);
	}

    HAL_Delay(1000);
}

//Auto/Self-Timed Mode Initialization Function
void Auto_Mode()
{
    //1.)Initialization
    //Disable the PS
    VCNL4035X01_SET_PS_SD(VCNL4035X01_PS_SD_DIS);
    //Setting up Auto Mode
    VCNL4035X01_SET_PS_AF(VCNL4035X01_PS_AUTO);
    //PS_SD will be set before measuring the offset or after all of the parameters have been set up

    //2.) Setting up PS
    //PS_CONF1
    //Set the PS IRED on/off Duty Ratio setting
    VCNL4035X01_SET_PS_DUTY(VCNL4035X01_PS_DR_1_40);
    //Set the Persistence
    VCNL4035X01_SET_PS_PERS(VCNL4035X01_PS_PERS_1);
    //Set the Integration Time
    VCNL4035X01_SET_PS_IT(PS_IT);

    //PS_CONF2
    //Enable/Disable the Gesture Interrupt
    VCNL4035X01_SET_GESTURE_INT(VCNL4035X01_PS_GESTURE_INT_DIS);
    //Enable/Disable the Gesture Mode
    VCNL4035X01_SET_GESTURE_MODE(VCNL4035X01_PS_GESTURE_MODE_DIS);
    //Set the Gain
    VCNL4035X01_SET_PS_Gain(VCNL4035X01_PS_GAIN_2_STEP);
    //Set the Output Bit Size
    VCNL4035X01_SET_PS_HD(VCNL4035X01_PS_HD_16Bits);
    //Set the Sensitivity Mode
    VCNL4035X01_SET_PS_NS(VCNL4035X01_PS_NS_2STEP_MODE_X4);
    //Set the Interrupt
    VCNL4035X01_SET_PS_INT(VCNL4035X01_PS_INT_CLOSE_AWAY);

    //PS_CONF3
    //Enable/Disable Low Current
    VCNL4035X01_SET_PS_LED_I_LOW(VCNL4035X01_PS_I_LOW_DIS);
    //Select IRED input to be Driven by the Internal Driver
    VCNL4035X01_SET_PS_IRED_select(IRED_Channel);
    //Enable/Disable Smart Persistence
    VCNL4035X01_SET_PS_SMART_PERS(VCNL4035X01_PS_SMART_PERS_DIS);
    //Set Interrupt to Normal/Logic Mode
    VCNL4035X01_SET_PS_MS(VCNL4035X01_PS_MS_NORMAL);
    //Enable/Disable Sunlight Cancellation
    VCNL4035X01_SET_PS_SC_EN(VCNL4035X01_PS_SC_EN);
    //PS_AF has been set during initialization
    //PS_TRIG must be set before reading the proximity data

    //PS_MS
    //Set the Sunlight Cancellation Current
    VCNL4035X01_SET_PS_SC_CUR(VCNL4035X01_PS_SC_CURx1);
    //Set the Sunlight Capability
    VCNL4035X01_SET_PS_SP(VCNL4035X01_PS_SPx1);
    //Set the Output of Sunlight Protect Mode
    VCNL4035X01_SET_PS_SPO(VCNL4035X01_PS_SPO_00);
    //Set the LED Current
    VCNL4035X01_SET_PS_LED_I(VCNL4035X01_PS_LED_I_100mA);

    //3.) Switch On the sensor
    VCNL4035X01_SET_PS_SD(VCNL4035X01_PS_SD_EN);
    //Delay of 10 ms needs to be changed depending on the API of the µ-controller of use
    HAL_Delay(10);

    //Clear Initial Interrupt
    VCNL4035X01_GET_PS_Interrupt();
    HAL_Delay(10);

	//4.) Threshold Setting and Offset Measurement
	for(int i=0;i<AverageCount;i++)
	{
		CalibValue += VCNL4035X01_READ_Reg(IRED_Channel_Offset_Read);
		//Delay of 10 ms needs to be changed depending on the API of the µ-controller of use
		HAL_Delay(10);
	}

	//Calculate the average of the offset measurement
	CalibValue /= AverageCount;

	//Check whether the offset value is too high to have been set in the cancellation register
	// 1T max offset value for the cancellation register is 2048
	if((PS_IT==VCNL4035X01_PS_IT_1T)||(PS_IT==VCNL4035X01_PS_IT_1_5T))Max_Offset = 2048;
	// 2T max offset value for the cancellation register is 4096
	if((PS_IT==VCNL4035X01_PS_IT_2T)||(PS_IT==VCNL4035X01_PS_IT_2_5T)||(PS_IT==VCNL4035X01_PS_IT_3T)||(PS_IT==VCNL4035X01_PS_IT_3_5T))Max_Offset = 4096;
	// 4T max offset value for the cancellation register is 8192
	if(PS_IT==VCNL4035X01_PS_IT_4T)Max_Offset = 8192;
	// 8T max offset value for the cancellation register is 16384
	if(PS_IT==VCNL4035X01_PS_IT_8T)Max_Offset = 16384;
	if(CalibValue>Max_Offset)
	{
		//Perform Offset Measurement
		if(SEL_Offset == 0) OffsetValue = 0;
		if(SEL_Offset == 1) OffsetValue = CalibValue;
		//Set Cancellation register to eliminate offset
		VCNL4035X01_SET_PS_CANC(OffsetValue);
		//Set Low Threshold
		VCNL4035X01_SET_PS_LowThreshold(3000 + OffsetValue);
		//set High Threshold
		VCNL4035X01_SET_PS_HighThreshold(5000 + OffsetValue);
	}
	else
	{
		//Perform Offset Measurement
		if(SEL_Offset == 0) OffsetValue = 0;
		if(SEL_Offset == 1) OffsetValue = CalibValue;
		//Set Cancellation register to eliminate offset
		VCNL4035X01_SET_PS_CANC(OffsetValue);
		//Set Low Threshold
		VCNL4035X01_SET_PS_LowThreshold(3000);
		//set High Threshold
		VCNL4035X01_SET_PS_HighThreshold(5000);
	}

	HAL_Delay(1000);
}
